#include "apps.h"

#define RETRY_INIT_DELAY 2000
#define RUN_DELAY 1000

void sc7a20_loop()
{
    static uint32_t last = 0;
    static uint32_t mark = 0;
    static int state = 0;
    int16_t x, y, z;

    if (millis() - last < 100)
    {
        return;
    }
    last = millis();
    switch (state)
    {
    case 0:
        if (accel.begin()) // Initialize the SC7A20
        {
            log_i("SC7A20 initialization successful");
            // 配置示例
            accel.setDataRate(0x50);           // 设置为 50Hz
            accel.setFullScale(0x10);          // 设置为 ±4g
            accel.enableHighPassFilter(true);  // 启用高通滤波器
            accel.setInterruptThreshold(0x10); // 设置中断阈值
            accel.setInterruptDuration(0x01);  // 设置中断持续时间
            accel.configureFIFO(0x02, 0x1F);   // FIFO 流模式，阈值31
            state = 2;                         // Set the state to 1 if initialization is successful
        }
        else
        {
            log_e("error: %d", accel.err);
            state = 1; // Set the state to 0 if initialization fails
        }
        mark = millis();
        break;
    case 1:
        if (millis() - mark < RETRY_INIT_DELAY)
        {
            break;
        }
        else
        {
            state = 0;
        }
    case 2:
        accel.readAcceleration(x, y, z);

        if (accel.err == 0)
        {
            state = 3;
            Serial.print("X: ");
            Serial.print(x);
            Serial.print(" Y: ");
            Serial.print(y);
            Serial.print(" Z: ");
            Serial.println(z);
            if(x==0&&y==0&&z==0)
            {
                log_e("error: %d", accel.err);
                state = 0;
            }
        }
        else
        {
            log_e("error: %d", accel.err); //(F() save string to flash & keeps dynamic memory free
            state = 0;
            break;
        }
        mark = millis();
        break;
    case 3:
        if (millis() - mark < RUN_DELAY) // 10 * 100ms = 1s
        {
            break;
        }
        else
        {
            state = 2;
        }
        break;

    default:
        state = 0;
        break;
    }
}